function [fusion] = fcn_fusion_dekf_22s_RW(gyro, accb, magb, ext_vel, ext_pos, stoch_p, q_initial, pos_initial, no_int, acc1g, dt)
%fcn_fusion_dekf_22s_RW   	This function uses an extended kalman filter.

%   [fusion] = fcn_fusion_dekf_22s_RW(gyro, accb, magb, ext_vel, ext_pos, stoch_p, q_initial, vel_initial, pos_initial, no_int, acc1g, dt)
%   EKF in direct formulation with 22 states: 
%   - q
%   - wt
%   - wb
%   - at
%   - ab
%   - v
%   - p

%   A Random Walk is used to model the gyro bias.

%   IN      gyro        Gyro measurements error-prone
%   IN      accb        Acc measurements error-prone
%   IN      magb        Mag measurements error-prone
%   IN      ext_vel     External velocity
%   IN      ext_pos     External position
%   IN      stoch_p     Vector containing stochastic parameters for error modells
%   IN      q_initial   Initial quaternion attitude
%   IN      pos_initial Initial position
%   IN      no_int      Number of samples before position integration starts (time for the algorithm to reach a steady state)
%   IN      acc1g       Gravity (equals the gravity at Analog Devices Calibration site)
%   IN      dt          Sample period
%   OUT     fusion      Fusioned data


%% Get QUEST initial attitude if wanted
if strcmpi(q_initial, 'QUEST')
	quest_noise = [stoch_p.Nacc; stoch_p.Nmag];
    obs = [ [accb(1,:)/norm(accb(1,:))]', [magb(1,:)/norm(magb(1,:))]'];
    ref = [[0, 0, 1]', [1, 0, 0]'];
    q_initial = quest(obs, ref, quest_noise, 'noise vectors', 1);    
end

%% For algorithm
% Get loop length
k = length(gyro);

% Save for output
% States
q_save   = zeros(k, 4);                         % Quaternion attitude
wt_save  = zeros(k, 3);                         % True gyroscope
wb_save  = zeros(k, 3);                         % Estimated gyro bias
at_save  = zeros(k, 3);                         % True acceleration
ab_save  = zeros(k, 3);                         % Estimated acc bias
vel_save = zeros(k, 3);                         % Velocity
pos_save = zeros(k, 3);                         % Position
% Kalman
F_save = zeros(k, 22, 22);                      % Jacobian of the state transition equation
P_save = zeros(k, 22, 22);                      % Error covariance matrix
% ACC update
h_acc_save = zeros(k, 3);                      	% Measurement equation of the ACC update
y_acc_save = zeros(k, 3);                      	% Errors of the ACC update
S_acc_save = zeros(k, 3, 3);                  	% Innovation covariance of the ACC update
K_acc_save = zeros(k, 22, 3);                 	% Kalman gain of the ACC update
% MAG update
h_mag_save = zeros(k, 3);                       % Measurement equation of the MAG update
y_mag_save = zeros(k, 3);                       % Errors of the MAG update
S_mag_save = zeros(k, 3, 3);                    % Innovation covariance of the MAG update
K_mag_save = zeros(k, 22, 3);               	% Kalman gain of the MAG update
% external update
h_ext_save = zeros(k, 6);                      	% Measurement equation of external velocity and position update
y_ext_save = zeros(k, 6);                      	% Errors of external velocity and position update
S_ext_save = zeros(k, 6, 6);                  	% Innovation covariance of external velocity and position update
K_ext_save = zeros(k, 22, 6);                 	% Kalman gain of external velocity and position update

% Kalman initials
% Attitude
q = q_initial;

% Gyroscope values (w_measured = w_true + w_bias)
wm = zeros(1, 3);
wt = zeros(1, 3);
wb = zeros(1, 3);

% Accelerometer values (a_measured = a_true + a_bias)
am = zeros(1, 3);
at = zeros(1, 3);
ab = zeros(1, 3);
g = [0; 0; acc1g];

% Magnetometer values
mm = zeros(1, 3);

% Velocity
v = zeros(1, 3);
vm = zeros(1, 3);

% Position
p = pos_initial; 
pm = zeros(1, 3);

%% For process noise covariance matrices
% Gyro
Q_WN_gyro = stoch_p.Ngyro^2/dt;
Q_GM_gyro = stoch_p.Bgyro^2;
Q_RW_gyro = stoch_p.Kgyro^2 * dt;
% For GM process:
Tc = (stoch_p.tc1_gyro + stoch_p.tc2_gyro) / 2;
tau = 1/Tc;
sigma = sqrt(2*log(2)/pi) * stoch_p.Bgyro;
% digital driving noise
Q_GM_DDN = (sigma^2 * (1-exp(-2*(dt/Tc))));

% Acc
Q_WN_acc = stoch_p.Nacc^2/dt;
Q_GM_acc = stoch_p.Bacc^2;
Q_RW_acc = stoch_p.Kacc^2 * dt;

% Mag
Q_WN_mag = stoch_p.Nmag^2/dt;
Q_GM_mag = stoch_p.Bmag^2;
Q_RW_mag = stoch_p.Kmag^2 * dt;

% Vel
Q_vel = stoch_p.sigma_ext_vel^2;

% Pos
Q_pos = stoch_p.sigma_ext_pos^2;

%% Initial matrices
% Initial error covariance matrix
P = zeros(22,22);  

% Process covariance matrix
Q = diag([Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_WN_gyro, Q_RW_gyro, Q_RW_gyro, Q_RW_gyro, Q_WN_acc, Q_WN_acc, Q_WN_acc, Q_RW_acc, Q_RW_acc, Q_RW_acc, Q_vel, Q_vel, Q_vel, Q_pos, Q_pos, Q_pos], 0);

% Measurement noise covariance matrix for ACC update
R_acc = diag([Q_WN_acc, Q_WN_acc, Q_WN_acc], 0);

% Measurement noise covariance matrix for MAG update
R_mag = diag([Q_WN_mag, Q_WN_mag, Q_WN_mag], 0);

% Measurement noise covariance matrix for external update
R_ext = diag([Q_vel, Q_vel, Q_vel, Q_pos, Q_pos, Q_pos], 0);

for j=1:k,j; 
    
    %% Get quaternion matrices for easier calculation
    C = [0        -q(4)     q(3);
         q(4)     0         -q(2);
         -q(3)    q(2)      0];

    q_mat = [ q(1)                  [-q(2) -q(3) -q(4)];
            [ q(2); q(3); q(4)]       q(1)*eye(3) + C];

    q_mat_conjugate = [ q(1)                  [-q(2) -q(3) -q(4)];
                      [ q(2); q(3); q(4)]       q(1)*eye(3) - C];

    % Rotationmatrix body -> inertial
    R_mat = transpose(q_mat_conjugate)*q_mat;
    R_mat = R_mat(2:4, 2:4);

    % Rotationmatrix inertial -> body
    R_mat_conjugate = q_mat_conjugate*transpose(q_mat);      
    R_mat_conjugate = R_mat_conjugate(2:4, 2:4);   
    
    
    %% Extended Kalman Filter
    %% Prediction
    % Input
    wm = gyro(j,:);
    wt = (wm-wb)';
    theta = norm(wt*dt);
    
    am = accb(j,:);
    at = R_mat*(am-ab)'-g;
    
    % Got to pass if theta is zero! otherwise we'd end up with NaN q's
    if (theta)
        % State transition equation
        f = [   q(1)*cos(theta/2) - (dt*q(2)*wt(1)*sin(theta/2))/theta - (dt*q(3)*wt(2)*sin(theta/2))/theta - (dt*q(4)*wt(3)*sin(theta/2))/theta;
                q(2)*cos(theta/2) + (dt*q(1)*wt(1)*sin(theta/2))/theta + (dt*q(4)*wt(2)*sin(theta/2))/theta - (dt*q(3)*wt(3)*sin(theta/2))/theta;
                q(3)*cos(theta/2) - (dt*q(4)*wt(1)*sin(theta/2))/theta + (dt*q(1)*wt(2)*sin(theta/2))/theta + (dt*q(2)*wt(3)*sin(theta/2))/theta;
                q(4)*cos(theta/2) + (dt*q(3)*wt(1)*sin(theta/2))/theta - (dt*q(2)*wt(2)*sin(theta/2))/theta + (dt*q(1)*wt(3)*sin(theta/2))/theta;
                wm(1) - wb(1);
                wm(2) - wb(2);
                wm(3) - wb(3);
                wb(1);
                wb(2);
                wb(3);
                (ab(2) - am(2))*(2*q(1)*q(4) - 2*q(2)*q(3)) - (ab(1) - am(1))*(q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2) - (ab(3) - am(3))*(2*q(1)*q(3) + 2*q(2)*q(4));
                (ab(3) - am(3))*(2*q(1)*q(2) - 2*q(3)*q(4)) - (ab(1) - am(1))*(2*q(1)*q(4) + 2*q(2)*q(3)) - (ab(2) - am(2))*(q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2);
                (ab(1) - am(1))*(2*q(1)*q(3) - 2*q(2)*q(4)) - (ab(3) - am(3))*(q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2) - acc1g - (ab(2) - am(2))*(2*q(1)*q(2) + 2*q(3)*q(4));
                ab(1);
                ab(2);
                ab(3);
                v(1) + at(1)*dt;
                v(2) + at(2)*dt;
                v(3) + at(3)*dt;
                (at(1)*dt^2)/2 + v(1)*dt + p(1);
                (at(2)*dt^2)/2 + v(2)*dt + p(2);
                (at(3)*dt^2)/2 + v(3)*dt + p(3)];

        % Jacobian of the state transition equation 
        F = [	cos(theta/2),                                                                   -(dt*wt(1)*sin(theta/2))/theta,                                                 -(dt*wt(2)*sin(theta/2))/theta,                                                 -(dt*wt(3)*sin(theta/2))/theta,                                                 -(dt*q(2)*sin(theta/2))/theta,  -(dt*q(3)*sin(theta/2))/theta,  -(dt*q(4)*sin(theta/2))/theta,  0,  0,  0,      0,      0,      0,      0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                (dt*wt(1)*sin(theta/2))/theta,                                                  cos(theta/2),                                                                   -(dt*wt(3)*sin(theta/2))/theta,                                                 (dt*wt(2)*sin(theta/2))/theta,                                                  (dt*q(1)*sin(theta/2))/theta,   (dt*q(4)*sin(theta/2))/theta,   -(dt*q(3)*sin(theta/2))/theta,  0,  0,  0,      0,      0,      0,      0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                (dt*wt(2)*sin(theta/2))/theta,                                                  (dt*wt(3)*sin(theta/2))/theta,                                                  cos(theta/2),                                                                   -(dt*wt(1)*sin(theta/2))/theta,                                                 -(dt*q(4)*sin(theta/2))/theta,  (dt*q(1)*sin(theta/2))/theta,   (dt*q(2)*sin(theta/2))/theta,   0,  0,  0,      0,      0,      0,      0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                (dt*wt(3)*sin(theta/2))/theta,                                                  -(dt*wt(2)*sin(theta/2))/theta,                                                 (dt*wt(1)*sin(theta/2))/theta,                                                  cos(theta/2),                                                                   (dt*q(3)*sin(theta/2))/theta,   -(dt*q(2)*sin(theta/2))/theta,  (dt*q(1)*sin(theta/2))/theta,   0,  0,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                           	0,                             -1,  0,  0,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0, -1,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0, -1,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              1,  0,  0,      0,      0,      0,     	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  1,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  1,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                2*q(4)*(ab(2) - am(2)) - 2*q(1)*(ab(1) - am(1)) - 2*q(3)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)),     2*q(3)*(ab(1) - am(1)) - 2*q(2)*(ab(2) - am(2)) - 2*q(1)*(ab(3) - am(3)),       2*q(4)*(ab(1) - am(1)) + 2*q(1)*(ab(2) - am(2)) - 2*q(2)*(ab(3) - am(3)),       0,                              0,                              0,                              0,  0,  0,      0,      0,      0,    	- q(1)^2 - q(2)^2 + q(3)^2 + q(4)^2, 	2*q(1)*q(4) - 2*q(2)*q(3),              - 2*q(1)*q(3) - 2*q(2)*q(4),            0,  0,  0, 0, 0, 0;
                2*q(2)*(ab(3) - am(3)) - 2*q(1)*(ab(2) - am(2)) - 2*q(4)*(ab(1) - am(1)),       2*q(2)*(ab(2) - am(2)) - 2*q(3)*(ab(1) - am(1)) + 2*q(1)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)),     2*q(4)*(ab(2) - am(2)) - 2*q(1)*(ab(1) - am(1)) - 2*q(3)*(ab(3) - am(3)),       0,                              0,                              0,                              0,  0,  0,      0,      0,      0,      - 2*q(1)*q(4) - 2*q(2)*q(3),            - q(1)^2 + q(2)^2 - q(3)^2 + q(4)^2,  	2*q(1)*q(2) - 2*q(3)*q(4),              0,  0,  0, 0, 0, 0;
                2*q(3)*(ab(1) - am(1)) - 2*q(2)*(ab(2) - am(2)) - 2*q(1)*(ab(3) - am(3)),       2*q(2)*(ab(3) - am(3)) - 2*q(1)*(ab(2) - am(2)) - 2*q(4)*(ab(1) - am(1)),       2*q(1)*(ab(1) - am(1)) - 2*q(4)*(ab(2) - am(2)) + 2*q(3)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)), 	0,                              0,                              0,                              0,  0,  0,      0,      0,      0,  	2*q(1)*q(3) - 2*q(2)*q(4),              - 2*q(1)*q(2) - 2*q(3)*q(4),            - q(1)^2 + q(2)^2 + q(3)^2 - q(4)^2,    0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,   	1,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                          	0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,     	0,                                      1,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,    	0,                                      0,                                      1,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,     dt,      0,      0,   	0,                                      0,                                      0,                                      1,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,     dt,      0,  	0,                                      0,                                      0,                                      0,  1,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                             	0,  0,  0,      0,      0,     dt,    	0,                                      0,                                      0,                                      0,  0,  1, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0, dt^2/2,      0,      0,    	0,                                      0,                                      0,                                      dt,  0,  0, 1, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0, dt^2/2,      0,    	0,                                      0,                                      0,                                      0, dt,  0, 0, 1, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0, dt^2/2,    	0,                                      0,                                      0,                                      0,  0, dt, 0, 0, 1];
    else
        f = [	q(1);
                q(2);
                q(3);
                q(4);
                wm(1) - wb(1);
                wm(2) - wb(2);
                wm(3) - wb(3);
                wb(1);
                wb(2);
                wb(3);
                (ab(2) - am(2))*(2*q(1)*q(4) - 2*q(2)*q(3)) - (ab(1) - am(1))*(q(1)^2 + q(2)^2 - q(3)^2 - q(4)^2) - (ab(3) - am(3))*(2*q(1)*q(3) + 2*q(2)*q(4));
                (ab(3) - am(3))*(2*q(1)*q(2) - 2*q(3)*q(4)) - (ab(1) - am(1))*(2*q(1)*q(4) + 2*q(2)*q(3)) - (ab(2) - am(2))*(q(1)^2 - q(2)^2 + q(3)^2 - q(4)^2);
                (ab(1) - am(1))*(2*q(1)*q(3) - 2*q(2)*q(4)) - (ab(3) - am(3))*(q(1)^2 - q(2)^2 - q(3)^2 + q(4)^2) - acc1g - (ab(2) - am(2))*(2*q(1)*q(2) + 2*q(3)*q(4));
                ab(1);
                ab(2);
                ab(3);
                v(1) + at(1)*dt;
                v(2) + at(2)*dt;
                v(3) + at(3)*dt;
                (at(1)*dt^2)/2 + v(1)*dt + p(1);
                (at(2)*dt^2)/2 + v(2)*dt + p(2);
                (at(3)*dt^2)/2 + v(3)*dt + p(3)];

        F = [   1,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                           	0,                              0,                              0,  0,  0,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              1,                                                                              0,                                                                              0,                                                                             	0,                              0,                              0,                              0,  0,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              1,                                                                              0,                                                                              0,                              0,                           	0,                              0,  0,  0,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              1,                                                                            	0,                              0,                              0,                              0,  0,  0,      0,      0,      0,  	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                           	0,                             -1,  0,  0,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0, -1,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0, -1,      0,      0,      0,    	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              1,  0,  0,      0,      0,      0,     	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  1,  0,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  1,      0,      0,      0,   	0,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                2*q(4)*(ab(2) - am(2)) - 2*q(1)*(ab(1) - am(1)) - 2*q(3)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)),     2*q(3)*(ab(1) - am(1)) - 2*q(2)*(ab(2) - am(2)) - 2*q(1)*(ab(3) - am(3)),       2*q(4)*(ab(1) - am(1)) + 2*q(1)*(ab(2) - am(2)) - 2*q(2)*(ab(3) - am(3)),       0,                              0,                              0,                              0,  0,  0,      0,      0,      0,    	- q(1)^2 - q(2)^2 + q(3)^2 + q(4)^2, 	2*q(1)*q(4) - 2*q(2)*q(3),              - 2*q(1)*q(3) - 2*q(2)*q(4),            0,  0,  0, 0, 0, 0;
                2*q(2)*(ab(3) - am(3)) - 2*q(1)*(ab(2) - am(2)) - 2*q(4)*(ab(1) - am(1)),       2*q(2)*(ab(2) - am(2)) - 2*q(3)*(ab(1) - am(1)) + 2*q(1)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)),     2*q(4)*(ab(2) - am(2)) - 2*q(1)*(ab(1) - am(1)) - 2*q(3)*(ab(3) - am(3)),       0,                              0,                              0,                              0,  0,  0,      0,      0,      0,      - 2*q(1)*q(4) - 2*q(2)*q(3),            - q(1)^2 + q(2)^2 - q(3)^2 + q(4)^2,  	2*q(1)*q(2) - 2*q(3)*q(4),              0,  0,  0, 0, 0, 0;
                2*q(3)*(ab(1) - am(1)) - 2*q(2)*(ab(2) - am(2)) - 2*q(1)*(ab(3) - am(3)),       2*q(2)*(ab(3) - am(3)) - 2*q(1)*(ab(2) - am(2)) - 2*q(4)*(ab(1) - am(1)),       2*q(1)*(ab(1) - am(1)) - 2*q(4)*(ab(2) - am(2)) + 2*q(3)*(ab(3) - am(3)),       - 2*q(2)*(ab(1) - am(1)) - 2*q(3)*(ab(2) - am(2)) - 2*q(4)*(ab(3) - am(3)), 	0,                              0,                              0,                              0,  0,  0,      0,      0,      0,  	2*q(1)*q(3) - 2*q(2)*q(4),              - 2*q(1)*q(2) - 2*q(3)*q(4),            - q(1)^2 + q(2)^2 + q(3)^2 - q(4)^2,    0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,   	1,                                      0,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                          	0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,     	0,                                      1,                                      0,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0,      0,    	0,                                      0,                                      1,                                      0,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,     dt,      0,      0,   	0,                                      0,                                      0,                                      1,  0,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,     dt,      0,  	0,                                      0,                                      0,                                      0,  1,  0, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                             	0,  0,  0,      0,      0,     dt,    	0,                                      0,                                      0,                                      0,  0,  1, 0, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0, dt^2/2,      0,      0,    	0,                                      0,                                      0,                                      dt,  0,  0, 1, 0, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0, dt^2/2,      0,    	0,                                      0,                                      0,                                      0, dt,  0, 0, 1, 0;
                0,                                                                              0,                                                                              0,                                                                              0,                                                                              0,                              0,                              0,                              0,  0,  0,      0,      0, dt^2/2,    	0,                                      0,                                      0,                                      0,  0, dt, 0, 0, 1];
    end
    
	% Predict states
    x = f;
    vp = x(17:19);
    pp = x(20:22);
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4));   

    % Predict error covariance
    P = F*P*F' + Q;
        
    
    %% Correction #1: based on ACC update
    % Measurements
    am = accb(j,:);
    
    % Normalize vectors for attitude determination
    am_n = am / norm(am); 
    
	% Measurement vector
	z_acc = [am_n]';
    
    % Measurement equation
    h_acc = [   R_mat_conjugate * [0 0 1]'];     
       
    % Jacobian of the measurement equation
    H_acc =  [  -2*q(3),                    2*q(4),                     -2*q(1),                    2*q(2),                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
                2*q(2),                     2*q(1),                     2*q(4),                     2*q(3),                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
                2*q(1),                     -2*q(2),                    -2*q(3),                    2*q(4),                     0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
         
    % Innovation
    y_acc = z_acc - h_acc;    
            
    % Innovation covariance (residual)
    S_acc = H_acc*P*H_acc' + R_acc;
        
    % Compute Kalman gain
    K_acc = P*H_acc' * inv(S_acc);    
      
    % Update the estimate (measurement residual)
    x = x + K_acc * y_acc;
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4)); 
    
    % Update error covariance
    P = (eye(22) - K_acc*H_acc) * P;
    
    
    %% Correction #2: based on MAG update
    % Measurements
    mm = magb(j,:);  
    
    % Normalize vectors for attitude determination
    mm_n = mm / norm(mm);    
    
	% Measurement vector
	z_mag = [mm_n]';
    
    % Measurement equation
    % Madgwick Magnetic Distortion
    mm_ni = R_mat * mm_n';                                      % body -> inertial
    b = [sqrt(mm_ni(1)^2 + mm_ni(2)^2); 0; mm_ni(3)];           % turn vector north
       
    h_mag = [   R_mat_conjugate * b];     
       
    % Jacobian of the measurement equation
    H_mag =  [  2*b(1)*q(1) - 2*b(3)*q(3),  2*b(1)*q(2) + 2*b(3)*q(4), -2*b(1)*q(3) - 2*b(3)*q(1),  2*b(3)*q(2) - 2*b(1)*q(4),  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
                2*b(3)*q(2) - 2*b(1)*q(4),  2*b(1)*q(3) + 2*b(3)*q(1), 	2*b(1)*q(2) + 2*b(3)*q(4),  2*b(3)*q(3) - 2*b(1)*q(1),  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
                2*b(1)*q(3) + 2*b(3)*q(1),  2*b(1)*q(4) - 2*b(3)*q(2),	2*b(1)*q(1) - 2*b(3)*q(3),  2*b(1)*q(2) + 2*b(3)*q(4),  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]; 
    
    % Innovation
    y_mag = z_mag - h_mag;
    
    % Innovation covariance (residual)
    S_mag = H_mag*P*H_mag' + R_mag;
        
    % Compute Kalman gain
    K_mag = P*H_mag' * inv(S_mag);    
      
    % Update the estimate (measurement residual)
    x = x + K_mag * y_mag;
    
    % Norm quaternion after manipulation
    x(1:4) = x(1:4) / norm(x(1:4)); 
    
    % Update error covariance
    P = (eye(22) - K_mag*H_mag) * P;
    
    
    %% Correction #3: based on external velocity and position update
    if (any(ext_vel(j,:)) || any(ext_pos(j,:)))
        % Measurements
        vm = ext_vel(j,:);
        pm = ext_pos(j,:);
        
        % Measurement vector
        z_ext = [vm, pm]';
        
    	% Measurement equation
        h_ext = [   vp;
                    pp];
                
        % Jacobian of the measurement equation                
        H_ext = [   0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0;
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0;
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0;
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0;
                    0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1];

        % Innovation
        y_ext = z_ext - h_ext;        
                
        % Innovation covariance (residual)
        S_ext = H_ext*P*H_ext' + R_ext;

        % Compute Kalman gain
        K_ext = P*H_ext' * inv(S_ext);    

        % Update the estimate (measurement residual)
        x = x + K_ext * y_ext;

        % Norm quaternion after manipulation
        x(1:4) = x(1:4) / norm(x(1:4)); 

        % Update error covariance
        P = (eye(22) - K_ext*H_ext) * P;       
        
        % Save Kalman variables
        h_ext_save(j,:) = h_ext';
        S_ext_save(j,:,:) = S_ext;
        y_ext_save(j,:) = y_ext';
        K_ext_save(j,:,:) = K_ext;
    end
    
    % Extract states from state vector
    q = x(1:4)';
    wt = x(5:7)';
    wb = x(8:10)';
    at = x(11:13)';
    ab = x(14:16)';
    v = x(17:19)';
    p = x(20:22)';
    
    % Save states
	q_save(j,:)  = q;
    wt_save(j,:) = wt;
    wb_save(j,:) = wb;
    at_save(j,:) = at;
    ab_save(j,:) = ab;
    vel_save(j,:) = v;
    pos_save(j,:) = p;
    
    % Save Kalman variables
    F_save(j,:,:) = F;
    P_save(j,:,:) = P;
    h_acc_save(j,:) = h_acc';
    S_acc_save(j,:,:) = S_acc;
    y_acc_save(j,:) = y_acc';
    K_acc_save(j,:,:) = K_acc;
    h_mag_save(j,:) = h_mag';
    S_mag_save(j,:,:) = S_mag;
    y_mag_save(j,:) = y_mag';
    K_mag_save(j,:,:) = K_mag;    

end


%% Output
% Eulerangles from quaternion attitude
[yaw, pitch, roll] = quat2angle( q_save, 'ZYX' );

% Output sample time
for j=1:k,j;
    fusion.time(j,1) = j*dt;
end

% Output position
fusion.X = pos_save(:,1);
fusion.Y = pos_save(:,2);
fusion.Z = pos_save(:,3);
fusion.pos = pos_save;

% Output velocity
fusion.vel = vel_save;
fusion.v_xy = (vel_save(:,1).^2 + vel_save(:,2).^2).^(1/2);

% Output acceleration
fusion.a_xy = (at_save(:,1).^2 + at_save(:,2).^2).^(1/2);

% Output euler angles
fusion.phi = roll;
fusion.theta = pitch;
fusion.psi = yaw;

% Output quaternion attitude
fusion.q = q_save;
% Output quaternion attitude where position integration starts
fusion.q_int = q_save(no_int+1,:);

% Output rest of states
fusion.wt = wt_save;
fusion.wb = wb_save;
fusion.at = at_save;
fusion.ab = ab_save;

% Output Kalman variables
fusion.F = F_save;
fusion.P = P_save;
fusion.h_acc = h_acc_save;
fusion.y_acc = y_acc_save;
fusion.S_acc = S_acc_save;
fusion.K_acc = K_acc_save;
fusion.h_mag = h_mag_save;
fusion.y_mag = y_mag_save;
fusion.S_mag = S_mag_save;
fusion.K_mag = K_mag_save;
fusion.h_ext = h_ext_save;
fusion.y_ext = y_ext_save;
fusion.S_ext = S_ext_save;
fusion.K_ext = K_ext_save;

% Output direction vectors: acceleration
fusion.acci = at_save;


end

